#include "ros/ros.h"
#include "std_msgs/String.h"

#include <serial/serial.h>
#include "SCServo.h"

serial::Serial ser;
SCServo SERVO;
u8 ID0[1] = {0};
u8 ID1[1] = {1};

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
	// SERVO.EnableTorque(0 ,1);
	// SERVO.EnableTorque(1 ,1);
	ROS_ERROR_STREAM("chatterCallback1");
	SERVO.SyncWritePos(ID0, 1, 1023, 500);
	// SERVO.RegWritePos(0, 1023, 500);
	ROS_ERROR_STREAM("chatterCallback1.2");
	// SERVO.RegWritePos(1, 250, 500);
	SERVO.SyncWritePos(ID1, 1, 230, 500);
	ROS_ERROR_STREAM("chatterCallback1.3");
	// SERVO.RegWriteAction();
	ROS_ERROR_STREAM("chatterCallback2");
}

void freeCallback(const std_msgs::String::ConstPtr& msg)
{
	// SERVO.EnableTorque(0 ,0);
	// SERVO.EnableTorque(1 ,0);
	// SERVO.SyncWritePos(ID, 2, 1023, 2000);
	SERVO.RegWritePos(0, 500, 2000);
	SERVO.RegWritePos(1, 710, 2000);
	SERVO.RegWriteAction();
}

int main(int argc, char *argv[])
{
	/**
	 * The ros::init() function needs to see argc and argv so that it can perform
	 * any ROS arguments and name remapping that were provided at the command line.
	 * For programmatic remappings you can use a different version of init() which takes
	 * remappings directly, but for most command-line programs, passing argc and argv is
	 * the easiest way to do it.  The third argument to init() is the name of the node.
	 *
	 * You must call one of the versions of ros::init() before using any other
	 * part of the ROS system.
	 */
	ros::init(argc, argv, "hand");

	std::string port;
	int baud;

	ros::param::get("~port", port);
	ros::param::get("~baud", baud);

	// ROS_ERROR_STREAM(port);
	// ROS_ERROR_STREAM(baud);
	
	try {
		//设置串口属性，并打开串口 
		ser.setPort(port);
		ser.setBaudrate(baud); 
		serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
		ser.setTimeout(to);
		ser.open();
		SERVO.pSerial = &ser;
		SERVO.EnableTorque(0, 1);
		SERVO.EnableTorque(1, 1);
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port ");
		return -1;
    }



	/**
	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
	 * NodeHandle destructed will close down the node.
	 */
	ros::NodeHandle n;

	/**
	 * The subscribe() call is how you tell ROS that you want to receive messages
	 * on a given topic.  This invokes a call to the ROS
	 * master node, which keeps a registry of who is publishing and who
	 * is subscribing.  Messages are passed to a callback function, here
	 * called chatterCallback.  subscribe() returns a Subscriber object that you
	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
	 * object go out of scope, this callback will automatically be unsubscribed from
	 * this topic.
	 *
	 * The second parameter to the subscribe() function is the size of the message
	 * queue.  If messages are arriving faster than they are being processed, this
	 * is the number of messages that will be buffered up before beginning to throw
	 * away the oldest ones.
	 */
	ros::Subscriber sub = n.subscribe("handup", 1000, chatterCallback);
	ros::Subscriber free_sub = n.subscribe("handfree", 1000, freeCallback);

	/**
	 * ros::spin() will enter a loop, pumping callbacks.  With this version, all
	 * callbacks will be called from within this thread (the main one).  ros::spin()
	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
	 */
	ros::spin();

	return 0;
}